//
// Created by liangwenhao on 2024/4/6.
//

#include "load_control.h"



Motor LoadMotor1,LoadMotor2,LoadMotor3,LoadMotor4,LoadMotor5;
int16_t Fire_Servo_Position = FireServoClosePosition;
float Load_CrossAngle;


uint8_t FiredFlag = 0;
uint8_t LoadMotor_AutoProcess = 0;
extern servo_t g_servo;

uint8_t Load_Init_Timer = 0;
uint8_t Load_Init_Flag = 0;
/**
 * @file: 传送带初始化
 */
void LoadMotor_Init()
{
    if(fabsf(LoadMotor1.Motor_Data.torque_current) < Load_InitMIN && fabsf(LoadMotor2.Motor_Data.torque_current) < Load_InitMIN)
    {
        Load_CrossAngle -= 100;
    }
    else
    {
        Load_Init_Timer++;
        if(Load_Init_Timer == 50)
        {
            LoadMotor1.PID_Angle.Angle_begin = LoadMotor1.PID_Angle.Real;
            LoadMotor2.PID_Angle.Angle_begin = LoadMotor2.PID_Angle.Real;
            Load_CrossAngle = 0;
            Load_Init_Timer = 0;
            Load_Init_Flag = 1;
        }
    }
}

float Load_CrossAngle_Last;
uint32_t JudgeTimer;
void LoadMotor_ShakeJudge()
{
    if(LoadMotor1.Flag.Shaking == 1 || LoadMotor2.Flag.Shaking == 1)
    {
        for(int16_t i=10;i>0;i--)
        {
            Load_CrossAngle+=(float)(100.0*i/4);
            osDelay(1);
        }
        for(int16_t i=-10;i<0;i++)
        {
            Load_CrossAngle+=(float)(100.0*i/4);
            osDelay(1);
        }
        LoadMotor1.Flag.Shaking = 0;
        LoadMotor2.Flag.Shaking = 0;
    }
    else
    {
        if(Load_CrossAngle_Last != Load_CrossAngle) /// 如果在运动则不用做防抖检测
        {
            Load_CrossAngle_Last = Load_CrossAngle;
            LoadMotor1.Flag.Shaking = 0;
            LoadMotor2.Flag.Shaking = 0;
            JudgeTimer = 0;
        }
        else
        {
            if(LoadMotor1.Motor_Data.real_speed != 0 || LoadMotor2.Motor_Data.real_speed != 0)
            {
                JudgeTimer++;
            }
            if(JudgeTimer == 300)
            {
                LoadMotor1.Flag.Shaking = 1;
                LoadMotor2.Flag.Shaking = 1;
            }
        }
    }

}

void FireServo_Control()
{
    if(RC_Ctl.rc.s2==RC_SW_UP){ // 手动模式
        if(RC_Ctl.rc.s1==RC_SW_UP && Load_CrossAngle == LOAD_Start)
        {
            Fire_Servo_Position = FireServoOpenPosition;
        }
        else if(RC_Ctl.rc.s1==RC_SW_MID && Load_CrossAngle == LOAD_Stage2)
        {
            Fire_Servo_Position = FireServoClosePosition;
        }
        else if(RC_Ctl.rc.s1==RC_SW_DOWN && Load_CrossAngle == LOAD_Start)
        {
            Fire_Servo_Position = FireServoOpenPosition;
        }
    }
    else if(RC_Ctl.rc.s2==RC_SW_MID)
    {
        if(RC_Ctl.rc.s1 != RC_Ctl_last.rc.s1)
        {
            Fire_Servo_Position = FireServoOpenPosition;
            osDelay(100);
            FiredFlag=1;
        }
    }
}


/**
 * @file: 传送带装填控制
 * @brief: 将滑台向后推至扳机处并锁住，传送带复位
 */
void LoadMotor_Control()
{
    if (fabsf(RC_Ctl.back_one.channel1) > 0.001f) {
        Load_CrossAngle += (float) RC_Ctl.back_one.channel1 * (-400);
        if(Load_CrossAngle < LOAD_Start) Load_CrossAngle = LOAD_Start;
        if(Load_CrossAngle > LOAD_Stage2) Load_CrossAngle = LOAD_Stage2;
    }
}
uint16_t Timercnt = 0;
void LoadMotor_AutoControl()
{

    Fire_Servo_Position = FireServoOpenPosition;
    if(LoadMotor_AutoProcess == 0)
    {
        if(Load_CrossAngle < LOAD_Stage1)
        {
            Load_CrossAngle += 400;
        }
        else
        {
            LoadMotor_AutoProcess = 1;
        }
    }
    else if(LoadMotor_AutoProcess == 1)
    {
        if(Load_CrossAngle < LOAD_Stage2)
        {
            Load_CrossAngle += 100;
        }
        if(Load_CrossAngle >= LOAD_Stage2)
        {
            Load_CrossAngle = LOAD_Stage2;
            LoadMotor_AutoProcess = 2;
        }
    }
    else if(LoadMotor_AutoProcess == 2)
    {
        Fire_Servo_Position = FireServoClosePosition;
        Timercnt++;
        if(Timercnt > 1500)
        {
            LoadMotor_AutoProcess = 3;
            Timercnt = 0;
        }

    }
    else if(LoadMotor_AutoProcess == 3)
    {
        if(Load_CrossAngle > LOAD_Start)
        {
            Fire_Servo_Position = FireServoClosePosition;
            Load_CrossAngle -= 400;
        }
        if(Load_CrossAngle <= LOAD_Start)
        {
            Load_CrossAngle = LOAD_Start;
            LoadMotor_AutoProcess = 4;
        }
    }

}
